#include "AstarSearch.h"
#include <map>
#include <iostream>
#include <utility>

using namespace Eigen;

int8_t* map_data;

#define inf 1>>20
struct GridNode;
typedef GridNode* GridNodePtr;

struct GridNode
{     
    int id;        // 1--> open set, -1 --> closed set
    Eigen::Vector2d coord; 
    Eigen::Vector2i dir;   // direction of expanding
    Eigen::Vector2i index;
	
    double gScore, fScore;
    GridNodePtr cameFrom;
    std::multimap<double, GridNodePtr>::iterator nodeMapIt;

    GridNode(Eigen::Vector2i _index, Eigen::Vector2d _coord){  
		id = 0;
		index = _index;
		coord = _coord;
		dir   = Eigen::Vector2i::Zero();

		gScore = inf;
		fScore = inf;
		cameFrom = NULL;
    }

    GridNode(){};
    ~GridNode(){};
};

GridNodePtr ** GridNodeMap;
Vector2i goalIdx;

float gl_xl=-25.6;
float gl_yl=-25.6;
float resolution=0.1;
float inv_resolution=1/resolution;
int GLX_SIZE=512;
int GLY_SIZE=512;
Vector2i coord2gridIndex(const Vector2d & pt) 
{
    Vector2i idx;
    idx <<  std::min( std::max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),
            std::min( std::max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1);             
  
    return idx;
}
Vector2d gridIndex2coord(const Vector2i & index) 
{
    Vector2d pt;

    pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;
    pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;

    return pt;
}

double getHeu(GridNodePtr node1, GridNodePtr node2)
{
    return (node1->coord-node2->coord).norm()*5;
}
inline bool isFree(const int & idx_x, const int & idx_y)
{
    return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && 
           (map_data[idx_y * GLX_SIZE + idx_x] <= 0));
}
inline bool isFree(const Eigen::Vector2i & index)
{
    return isFree(index(0), index(1));
}
inline void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets)
{   
    neighborPtrSets.clear();
    edgeCostSets.clear();
    Eigen::Vector2i neighborIdx;
    for (int i=-1;i<=1;i++)
    {
        for (int j=-1;j<=1;j++)
        {
            neighborIdx(0)=currentPtr->index(0)+i;
            neighborIdx(1)=currentPtr->index(1)+j;

            if(isFree(neighborIdx))
            {
                neighborPtrSets.push_back(GridNodeMap[neighborIdx(0)][neighborIdx(1)]);
            }
        }
    }
}

void AstarGraphSearch(Vector2d start_pt, Vector2d end_pt)
{
    std::multimap<double, GridNodePtr> openSet;

    Vector2i start_idx = coord2gridIndex(start_pt);
    Vector2i end_idx   = coord2gridIndex(end_pt);
    goalIdx = end_idx;

    start_pt = gridIndex2coord(start_idx);
    end_pt   = gridIndex2coord(end_idx);

    GridNodePtr startPtr = new GridNode(start_idx, start_pt);
    GridNodePtr endPtr   = new GridNode(end_idx,   end_pt);

    openSet.clear();

    GridNodePtr currentPtr  = NULL;
    GridNodePtr neighborPtr = NULL;

    startPtr -> gScore = 0;
    startPtr -> fScore = getHeu(startPtr,endPtr);  

    startPtr -> id = 1; 
    startPtr -> coord = start_pt;
    openSet.insert( std::make_pair(startPtr -> fScore, startPtr) );

    std::vector<GridNodePtr> neighborPtrSets;
    std::vector<double> edgeCostSets;
    GridNodePtr terminatePtr;
    while ( !openSet.empty() )
    {
        currentPtr=openSet.begin()->second;
        openSet.erase(openSet.begin());
        currentPtr->id=-1;

        if( currentPtr->index == goalIdx )
        {
            terminatePtr = currentPtr;          
            return;
        }
        AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets);      
        for(int i = 0; i < (int)neighborPtrSets.size(); i++)
        {
            neighborPtr=neighborPtrSets.begin()[i];
            if(neighborPtr -> id != 1 && neighborPtr -> id != -1)
            { 
                neighborPtr->gScore=currentPtr->gScore+1;
                neighborPtr->fScore=getHeu(neighborPtr,endPtr); 
                neighborPtr->cameFrom=currentPtr;
                neighborPtr->id=1;
                openSet.insert(std::make_pair(neighborPtr->fScore+neighborPtr->gScore,neighborPtr));
                continue;
            }
            else if(neighborPtr -> id == 1)
            {
                double tmp_gScore=currentPtr->gScore+1;
                double tmp_fScore=getHeu(neighborPtr,endPtr);
                if(neighborPtr->fScore+neighborPtr->gScore>tmp_gScore+tmp_fScore)
                {
                    neighborPtr->fScore=tmp_fScore;
                    neighborPtr->gScore=tmp_gScore;
                    std::multimap<double, GridNodePtr>::const_iterator begin_point=openSet.begin();
                    for(int point=0;point<openSet.size();point++)
                    {
                        if(begin_point->second == neighborPtr)
                        {
                            openSet.erase(begin_point);
                            openSet.insert(std::make_pair(neighborPtr->fScore+neighborPtr->gScore,neighborPtr));
                            break;
                        }
                        begin_point++;
                    }
                }
                continue;
            }
            else
            {
                continue;
            }
        }      
    }
}

std::vector<Eigen::Vector2d> getPath() 
{   
    std::vector<Vector2d> path;
    std::vector<GridNodePtr> gridPath;

    GridNodePtr currentNode=GridNodeMap[goalIdx(0)][goalIdx(1)];
    do
    {
        gridPath.push_back(currentNode);
        currentNode=currentNode->cameFrom;
    } while (currentNode!=NULL);
    
    for (auto ptr: gridPath)
        path.push_back(ptr->coord);
        
    reverse(path.begin(),path.end());

    return path;
}

void resetGrid(GridNodePtr ptr)
{
    ptr->id = 0;
    ptr->cameFrom = NULL;
    ptr->gScore = inf;
    ptr->fScore = inf;
}

void resetUsedGrids()
{   
    for(int i=0; i < GLX_SIZE ; i++)
        for(int j=0; j < GLY_SIZE ; j++)
                resetGrid(GridNodeMap[i][j]);
}


